#include "_pikabot_Bot.h"

#define FRAME_HEADER 0x55            
#define CMD_SERVO_MOVE 0x03           
#define CMD_ACTION_GROUP_RUN 0x06    
#define CMD_ACTION_GROUP_STOP 0x07   
#define CMD_ACTION_GROUP_SPEED 0x0B   
#define CMD_GET_BATTERY_VOLTAGE 0x0F  

#define GET_LOW_BYTE(A) ((uint8_t)(A))
#define GET_HIGH_BYTE(A) ((uint8_t)((A) >> 8))

size_t construct_instruction_packet(uint8_t servo_id, uint16_t angle, uint16_t time, uint8_t* packet) {
    uint8_t angle_low = angle & 0xFF;
    uint8_t angle_high = angle >> 8;
    uint8_t time_low = time & 0xFF;
    uint8_t time_high = time >> 8;

    packet[0] = 0x55;
    packet[1] = 0x55;
    packet[2] = 8;
    packet[3] = 0x03;
    packet[4] = 0x01;
    packet[5] = time_low;
    packet[6] = time_high;
		packet[7] = servo_id;
		packet[8] = angle_low;
    packet[9] = angle_high;
		return 10;
}


Arg* _pikabot_Bot_pack(PikaObj *self, int servo_id, int angle, int time){
		Arg* aBuf = arg_newBytes(NULL, 64);
		uint8_t* buf = arg_getBytes(aBuf);
		size_t len = construct_instruction_packet(servo_id, angle, time, buf);
		Arg* aRet = arg_newBytes(buf, len);
		arg_deinit(aBuf);
		return aRet;
}

typedef struct _lobot_servo_ {  
	uint8_t ID;
	uint16_t Position;
} LobotServo;

size_t construct_instruction_packets(LobotServo servos[], uint8_t Num, uint16_t Time, uint8_t* LobotTxBuf){
		uint32_t index = 7;
		uint32_t i = 0;

		if (Num < 1 || Num > 32 || !(Time > 0)) {
			return NULL;
		}
		LobotTxBuf[0] = LobotTxBuf[1] = FRAME_HEADER;      
		LobotTxBuf[2] = Num * 3 + 5;                       
		LobotTxBuf[3] = CMD_SERVO_MOVE;                   
		LobotTxBuf[4] = Num;                             
		LobotTxBuf[5] = GET_LOW_BYTE(Time);               
		LobotTxBuf[6] = GET_HIGH_BYTE(Time);               

		for (i = 0; i < Num; i++) {                       
			LobotTxBuf[index++] = servos[i].ID;              
			LobotTxBuf[index++] = GET_LOW_BYTE(servos[i].Position); 
			LobotTxBuf[index++] = GET_HIGH_BYTE(servos[i].Position);
		}
		return LobotTxBuf[2] + 2;
}

Arg* _pikabot_Bot_packs(PikaObj *self, PikaObj* id_positions, int time){
		uint32_t Num = objList_getSize(id_positions);
  	Arg* aBuf = arg_newBytes(NULL,64 + Num * sizeof(LobotServo));
	  uint8_t* buff = arg_getBytes(aBuf);
		LobotServo* lobotServo = pikaMalloc(Num * sizeof(LobotServo));
		for (int i = 0; i < Num; i++){
			  Arg* a_id_position = objList_get(id_positions, i);
				PikaObj* id_position = arg_getObj(a_id_position);
				Arg* a_id = objTuple_get(id_position, 0);
			  Arg* a_position = objTuple_get(id_position, 1);
				lobotServo[i].ID = arg_getInt(a_id);
				lobotServo[i].Position = arg_getInt(a_position);
			  arg_deinit(a_id);
			  arg_deinit(a_position);
			  arg_deinit(a_id_position);
		}
		size_t len = construct_instruction_packets(lobotServo, Num, time, buff);
		Arg* aRet = arg_newBytes(buff, len);
		arg_deinit(aBuf);
		pikaFree(lobotServo, Num * sizeof(LobotServo));
		return aRet;
}

